import struct
import time
import  pymodbus
from pymodbus.client import ModbusSerialClient as ModbusClient
import  serial
class Lift:

    def __init__(self):

        port = getSerialPort(0x1a86, 0x7523, None)
        self.client = ModbusClient(method='rtu', port=port , baudrate=9600)
        if self.client.connect():
            print("Modbus RTU Client Connected")
        else:
            print("Failed to connect to Modbus RTU Client")
        self.reset()
        self.pos = 0
        self.ratio = 10000 * 10 / 6.0
        self.aim_pos_motor = 0
        self.aim_pos_mm = 0

    def set_pos_noblock(self, aim_pos):
        if aim_pos > 480:
            aim_pos = 480
        if aim_pos < 0:
            aim_pos = 0
        self.aim_pos_motor = int(aim_pos * self.ratio)
        print(self.aim_pos_motor)
        self.client.write_registers(0x6201, [(self.aim_pos_motor & 0xFFFF0000) >> 16 , self.aim_pos_motor & 0x0000FFFF], 0x01)
        # self.client.write_registers(0x6201, [0x19, 0x6E6A], 0x01)
        self.client.write_register(0x6002,0x10,0x01)

    def set_pos_block(self, aim_pos):
        if aim_pos > 480:
            aim_pos = 480
        if aim_pos < 0:
           aim_pos = 0

        self.aim_pos_mm = aim_pos
        self.aim_pos_motor= int(aim_pos * self.ratio)

        print(self.aim_pos_motor)
        self.client.write_registers(0x6201, [(self.aim_pos_motor & 0xFFFF0000) >> 16 , self.aim_pos_motor & 0x0000FFFF], 0x01)
        # self.client.write_registers(0x6201, [0x19, 0x6E6A], 0x01)
        self.client.write_register(0x6002,0x10,0x01)
        while True:
            current_pos = self.get_pos()
            print('当前位置:',current_pos,'目标位置:',aim_pos)
            if abs(current_pos - self.aim_pos_mm) < 5:
                break
            time.sleep(1)

    def get_pos(self):
        re = self.client.read_holding_registers(0x602c, 2, 1)
        tmp1 = (re.registers[0])
        tmp2 = (re.registers[1])
        # tmp1 = (re.registers[0]) << 16
        # tmp2 = (re.registers[1])&0xFFFF
        # val = tmp1|tmp2

        import struct
        import struct

        # def combine_int16_to_int32(a: int, b: int) -> int:
        """
        将两个 int16 拼接成一个 int32（大端序，a 为高16位，b 为低16位）
        :param a: 第一个 int16 数（高16位）
        :param b: 第二个 int16 数（低16位）
        :return: 拼接后的 int32 数
        """
        # 打包为两个大端序的 int16（格式 '>hh' 表示大端序，两个 short）
        packed = struct.pack('>HH', tmp1, tmp2)
        # 解包为大端序的 int32（格式 '>i' 表示大端序，一个 int）
        val = struct.unpack('>l', packed)[0]
        self.pos = val/self.ratio
        return  self.pos

    def reset(self):
        self.client.write_register(0x600A,0x00,0x01)
        self.client.write_registers(0x600F, [0x0658, 0x012E], 0x01)
        self.client.write_register(0x6002,0x20,0x01)
        count = 0
        while True:
            if count > 200:
                return False
            count = count + 1
            re = self.client.read_holding_registers(0x0B05,1,1)
            if (re.registers[0] & 0x08) == 0x08:
                break
            time.sleep(0.5)

        self.client.write_register(0x6200,0x01,0x01)
        self.client.write_register(0x6203,0x0658,0x01)
        return  True

def getSerialPort(vid: int, pid: int, serial: object) -> str:
    from serial.tools import list_ports

    ports = list_ports.comports()
    for v in ports:
        if vid == v.vid and pid == v.pid and (not serial or serial == v.serial_number):
            return v.device
    return ""

if __name__ == '__main__':
    lift = Lift()
    lift.set_pos(200)
    time.sleep(10)
    print(lift.get_pos())

